/*************************************************
* @par Copyright (C): 2010-2019, hunan CLB Tech
* @file         car_server.c
* @version      V2.0
* @details
* @par History

@author: zhulin
***************************************************/
#include <signal.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>
#include <netdb.h>
#include <sys/types.h>
#include <time.h>
#include <sys/socket.h>
#include <arpa/inet.h>

#include <wiringPi.h>
#include <softPwm.h>

#include "car_move_pi4.h"
#include "car_control_pi4.h"
#define BUFSIZE 512

#define SINGLE_MSG_MAX_LEN 50
static char* CTL_MSG_END = "\r\n";
static int   END_MSG_LEN = 2;
void car_close()
{
    //System Exit
    printf("\r\nHandler:Motor Stop\r\n");
    Motor_Stop(MOTORA);
    Motor_Stop(MOTORB);
    Motor_Stop(MOTORC);
    Motor_Stop(MOTORD);
    DEV_ModuleExit();
}

/*******************舵机定义*************************
*****************************************************/
int myservo1 = 15; //舵机0 手爪
int myservo2 = 14; //舵机1 上臂
int myservo3 = 13; //舵机2 下臂
int myservo4 = 12; //舵机3 底座

int myservo5 = 10; //左右云台
int myservo6 = 9; //上下云台

//int SERVOS = 4;   //说明有4个舵机
float currentAngle[6];
float s_MIN[6];
float s_MAX[6];
float INITANGLE[6];

int flag = 0;            //标志位

#define ServoDelayTime 50
int delta = 5;  //舵机转动幅度
int delta_bottom = 2; //底座舵机转动幅度
int delta_yun = 10; //云台舵机转动幅度
/*******************舵机函数定义*************************
*********************************************************/
void car_init()
{
    
   // 电机初始化
    Motor_Init();
    
    //手爪舵机
    s_MIN[3] = 30;
    s_MAX[3] = 80;
    INITANGLE[3] = 75;
    
    //上臂舵机
    s_MIN[2] = 0;
    s_MAX[2] = 170;
    INITANGLE[2] = 0;
    
    //下臂舵机
    s_MIN[1] = 0;
    s_MAX[1] = 170;
    INITANGLE[1] = 95;
    
    //底座舵机
    s_MIN[0] = 0;
    s_MAX[0] = 176;
    INITANGLE[0] = 176;

    //摄像头云台舵机左右
    s_MIN[4] = 0;
    s_MAX[4] = 180;
    INITANGLE[4] = 80;

    //摄像头云台舵机上下
    s_MIN[5] = 0;
    s_MAX[5] = 180;
    INITANGLE[5] = 50;
    
    
    //初始化舵机    
    set_servo_angle(myservo1,INITANGLE[3]); //手爪舵机
    set_servo_angle(myservo2,INITANGLE[2]); //上臂舵机
    set_servo_angle(myservo3,INITANGLE[1]); //下臂舵机
    set_servo_angle(myservo4,INITANGLE[0]); //底座舵机
    
    set_servo_angle(myservo5,INITANGLE[4]); //云台左右控制
    set_servo_angle(myservo6,INITANGLE[5]); //云台上下控制
    
    currentAngle[0] = INITANGLE[0];  
    currentAngle[1] = INITANGLE[1];
    currentAngle[2] = INITANGLE[2];
    currentAngle[3] = INITANGLE[3];

    currentAngle[4] = INITANGLE[4];
    currentAngle[5] = INITANGLE[5];
}

void arm_hand_ctrl()                
{
    if(flag == 0){
        set_servo_angle(myservo1,s_MIN[3]); //手爪打开
        flag = 1;
    }else{
        set_servo_angle(myservo1,s_MAX[3]); //手爪闭合
        flag = 0;
    }
   delay(300);
  
   printf("arm open now pos:%d\n",currentAngle[3]);
}

void arm_bottom_ctrl(int postion)             // 底座
{
    if(postion < s_MIN[0])postion = s_MIN[0];
    if(postion > s_MAX[0])postion = s_MAX[0];
    currentAngle[0] = postion;
    if(postion == 0){
        if(currentAngle[0] + delta_bottom < s_MAX[0]) {
            currentAngle[0] += delta_bottom;
        }
    }
    if(postion == 255){
        if(currentAngle[0] - delta_bottom < s_MIN[0]) {
            currentAngle[0] -= delta_bottom;
        }
    }
    set_servo_angle(myservo4,currentAngle[0]);
    printf("arm bottom now pos:%d\n",currentAngle[0]);
}


void arm_up_ctrl(int postion)            //上臂舵机
{
    if(postion < s_MIN[2])postion = s_MIN[2];
    if(postion > s_MAX[2])postion = s_MAX[2];
    currentAngle[2] = postion;
    if(postion == 0){
        if(currentAngle[2] + delta < s_MAX[2]) {
            currentAngle[2] += delta;
        }
    }
    if(postion == 255){
        if(currentAngle[2] - delta < s_MIN[2]) {
            currentAngle[2] -= delta;
        }
    }
    set_servo_angle(myservo2,currentAngle[2]);
    printf("arm a now pos:%d\n",currentAngle[2]);
}


void arm_down_ctrl(int postion)     //下臂舵机
{
    if(postion < s_MIN[1])postion = s_MIN[1];
    if(postion > s_MAX[1])postion = s_MAX[1];
    currentAngle[1] = postion;
    if(postion == 0){
        if(currentAngle[1] + delta < s_MAX[1]) {
            currentAngle[1] += delta;
        }
    }
    if(postion == 255){
        if(currentAngle[1] - delta < s_MIN[1]) {
            currentAngle[1] -= delta;
        }
    }
    set_servo_angle(myservo3,currentAngle[1]);
    printf("arm b now pos:%d\n",currentAngle[1]);
}


void yun_lr_ctrl(int postion)     //云台左转
{
    if(postion < s_MIN[4])postion = s_MIN[4];
    if(postion > s_MAX[4])postion = s_MAX[4];
    currentAngle[4] = postion;
    if(postion == 0){
        if(currentAngle[4] + delta_yun < s_MAX[4]) {
            currentAngle[4] += delta_yun;
        }
    }
    if(postion == 255){
        if(currentAngle[4] - delta_yun < s_MIN[4]) {
            currentAngle[4] -= delta_yun;
        }
    }
    set_servo_angle(myservo3,currentAngle[4]);
    printf("arm b now pos:%d\n",currentAngle[4]);
}

void yun_ud_ctrl(int postion)     //云台上转
{
    if(postion < s_MIN[5])postion = s_MIN[5];
    if(postion > s_MAX[5])postion = s_MAX[5];
    currentAngle[5] = postion;
    if(postion == 0){
        if(currentAngle[5] + delta_yun < s_MAX[5]) {
            currentAngle[5] += delta_yun;
        }
    }
    if(postion == 255){
        if(currentAngle[5] - delta_yun < s_MIN[5]) {
            currentAngle[5] -= delta_yun;
        }
    }
    set_servo_angle(myservo3,currentAngle[5]);
    printf("arm b now pos:%d\n",currentAngle[5]);
}




void servo_stop() //停止所有舵机
{
  set_servo_angle(myservo1,currentAngle[3]);
  set_servo_angle(myservo2,currentAngle[2]);
  set_servo_angle(myservo3,currentAngle[1]);
  set_servo_angle(myservo4,currentAngle[0]);
}




//一组舵机动作接口
//多个舵机动作接口
/*
static char* CTL_MOTION_GROUP_MSG = "PL %d SQ %d SM %d IX %d";
static char* CTL_MOTION_GROUP_ONCE_MSG = "PL %d SQ %d SM %d IX %d ONCE";

int scene_code;             //动作场景编号，只用于标识，从0开始
int array_code;             //动作动编号0-127
int      speed;             //执行动作速度（-200 - 200），不指定则为100，即100%一培速，负数表示逆向运行动作
int start_code;             //指定启动动作组开始步编号，0-255
int       loop;             //是否循环运行
*/
int car_ctl_motion_group(motion_group_data * motion_group)
{
   return 0;
}

//多个舵机动作接口
/*
typedef struct motion_data_{
        int motor;                  //电机编号
        int postion;                //机械手电机转动到的位置
        int speed;                  //机载手转动的速度
        int time;                   //机械手电机转动到指定位置所需时间(单位毫秒)
}motion_data;
#define DEVICE_HAND_MOTORS                      0x100
//爪子
#define DEVICE_HAND_MOTOR_01                    0x101
//A
#define DEVICE_HAND_MOTOR_02                    0x102
//B
#define DEVICE_HAND_MOTOR_03                    0x103
//底
#define DEVICE_HAND_MOTOR_04                    0x104
//云台左右
#define DEVICE_HAND_MOTOR_05                    0x105
//去台上下
#define DEVICE_HAND_MOTOR_06                    0x106


 //手爪打开
void arm_hand_ctrl(int postion);  
// 底座左转
void arm_bottom_ctrl(int postion);   
 //上臂舵机
void arm_up_ctrl(int postion);      
//下臂舵机
void arm_down_ctrl(int postion);  
//云台左右转
void yun_lr_ctrl(int postion);
//云台上下转
void yun_ud_ctrl(int postion);

*/
int car_ctl_motions(motion_data * motions,int motion_size)
{
    int ret = 0,msg_len=0,i=1;
    if(!motions || motion_size < 1){
        dbg_info("car_ctl_motions error for motinos is null");
        return ERROR_PARAM;
    }
    while(i<motion_size)
    {   
        char templet[SINGLE_MSG_MAX_LEN]={0};
        int temp_len = 0 ;
        assert((motions+i)->motor>=DEVICE_HAND_MOTOR_01 && (motions+i)->motor<=DEVICE_HAND_MOTOR_06);
        switch((motions+i)->motor){
            case DEVICE_HAND_MOTOR_01:{
                arm_hand_ctrl();
            }break;
            case DEVICE_HAND_MOTOR_02:{
                arm_up_ctrl((motions+i)->postion);
            }break;
            case DEVICE_HAND_MOTOR_03:{
                arm_down_ctrl((motions+i)->postion);
            }break;
            case DEVICE_HAND_MOTOR_04:{
                arm_bottom_ctrl((motions+i)->postion);
            }break;
            case DEVICE_HAND_MOTOR_05:{
                yun_lr_ctrl((motions+i)->postion);
            }break;
            case DEVICE_HAND_MOTOR_06:{
                yun_ud_ctrl((motions+i)->postion);
            }break;
        }
        i++;
    }
   
    dbg_info("car_ctl_motions success，len:%d",motion_size);
    ret = SUCCESS;
    return ret;
}

void car_uninit()
{
    int ret = 0;
    car_close();
    //car_wheel_cfg* wheel = car->wheel;
    return ret;
}
